#include <Arduino.h>
#include "esp_timer.h"
#include "Queue.h"
#include "distance.h"
#include "mpu6050.h"
#include "bt.h"
#include "kalman.h"
#include "light.h"
#include "mwfilter.h"

/*
 d22 scl
 d21 sda
 d33 echo
 d32 trig
 d26 led high
*/
int Quest_State = 0;
KalmanFilter kf_g;
MovingAverageFilter mfilter(5); 
MovingAverageFilter dmfilter(5);
void setup() {
  Serial.begin(115200);
  Kalman_Init(&kf_g, 0.004f, 0.5f);
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  light_Init();
  Serial.println("hello");
  mpu6050_Init();
  InitBT();
  delay(100);
}

void testPrint();
#define DISTANCE_QUEUE_SIZE 24
int64_t high_time; //记录上一次的高点时间，以计算周期
Queue DistanceQueue(DISTANCE_QUEUE_SIZE), GyroQueue(6), DGQueue(6), TQueue(5), mindQueue(5), gmindQueue(5);
int64_t highpassTime = 0; //高点后的“不应期”
int64_t lowpassTime = 0; //低点后的“不应期”
int64_t ledOnTime = 0;
int64_t ledOffTime = 0;
bool isMotion = false; //是否正在运动
int maxd,mind; //距离的最大值和最小值（当底板水平时）
int global_mind = 10000; //全局最小值，用于测量底板倾角
int L = 0;
int T = 0;
int gm = 0;
int TCount = 0;
int rollCount = 0;
void loop() {
  // testPrint();
  // return;
  //>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>数据测量>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
  //测量距离，并保存队列
  double d = measureDistance();
  if (d < 10) { //观察到距离会有0数据的出现，故当出现0时，用上一次的数据代替
    d = DistanceQueue.getTop();
  }
  d = dmfilter.update(d); //滤波
  DistanceQueue.enqueue((int)d);
  global_mind = global_mind < d ? global_mind : (int)d; //记录全局最小值
  //测量角速度，进行卡尔曼滤波并保存队列（取x，y平面角速度）
  MpuData data = mpu6050_GetData();
  //double g = sqrt(data.gx * data.gx + data.gy * data.gy + data.gz * data.gz);
  double g = sqrt(data.gx * data.gx + data.gy * data.gy);
  g = (int)(Kalman_Update(&kf_g, g)*1000);
  GyroQueue.enqueue(g);
  //>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>数据处理>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
  //>>>>>>>>>>>>>>>>>>>>>>>(1)最高最低点的判定
  //首先，应获取稳定的周期，判断依据为5次半周期值的最大值和最小值之差小于200，取最后四次平均，播报蓝牙
  //周期稳定后不再测量，直到下次停止运动，并设周期稳定标志位
  //此时开始测量最低点距离与绳长。判断依据仍然是5次半周期距离的最大值和最小值之差小于25，取最后四次平均，保存之，以备后面使用，播报蓝牙
  //同时，应根据周期值修正亮灯的时间。在最高点时，让下次的低点前100ms亮灯；在最低点时，让下次的高点前后50ms亮灯
  int gdata[6]; //用以保存角速度队列数据
  int ddata[DISTANCE_QUEUE_SIZE]; //用于保存距离队列数据
  if(GyroQueue.isReady()) { //队列已满
    GyroQueue.getData(gdata); //将队列数据复制进数组中
    DistanceQueue.getData(ddata);
    bool AllBelow80 = true;
    bool AllBelow200 = true;
    for (int i = 0; i < GyroQueue.getSize(); i++) //判断是否正在运动，以角速度70为阈值
    {
      if(gdata[i] > 200) {
        AllBelow200 = false;
      }
      if(gdata[i] > 80) {
        AllBelow80 = false;
      } 
    }
    if(!isMotion ) {
      if(!AllBelow200) {
        isMotion = true;
        Serial.println("Motion Start");
        GetBT().println("Motion Start");
      } else {
        return;
      }
    }
    if(isMotion && AllBelow80) {
      isMotion = false;
      global_mind = 10000;
      T = 0;
      //L = 0;
      Serial.println("No Motion");
      GetBT().println("No Motion");
      light_Off();
      return;
    }
    //计算角速度的导数，并对导数进行窗口滤波，加入队列
    int dg = gdata[0]-gdata[2];
    dg = mfilter.update(dg);
    DGQueue.enqueue(dg);
    if (!DGQueue.isReady())
    {
      return;
    }
    int dgdata[6]; //用以保存角速度导数的队列数据
    DGQueue.getData(dgdata);
    int d1 = dgdata[0];
    int d2 = dgdata[1];
    int d3 = dgdata[2];
    int d4 = dgdata[3];
    d1 = d1+d2;
    d3 = d3+d4;
    int64_t now_time = esp_timer_get_time();
    // if(now_time > highpassTime && now_time > lowpassTime) { //判断时间以关灯
    //   light_Off();
    // }
    if(ledOnTime != 0 && now_time > ledOnTime) {
      light_On();
      ledOnTime = 0;
    }
    if(ledOffTime !=0 && now_time > ledOffTime) {
      light_Off();
      ledOffTime = 0;
    }
    if (d1 * d3 < 0 || d1 == 0 && d3 != 0 || d3 == 0 && d1 != 0) { //极值点
      if(d1 > 0 || d3 < 0) { //角速度极小值,摆的最高点
        if(now_time < highpassTime || highpassTime > lowpassTime) { //先有最低点再有最高点，不能连续两个最高点
          return;
        }
        highpassTime = now_time + 200*1000; //跳过判断的时间
        int64_t Td2 = now_time - high_time; //两次高点的时间差为周期的一半
        high_time = now_time;
        maxd = 0;  //取前几个距离点的最大值作为高点的距离
        for (int i = 0; i < DistanceQueue.getSize(); i++)
        {
          maxd = maxd < ddata[i] ? ddata[i] : maxd;
        }
        if(Quest_State == 1 && T > 0) {
          int rT = T + (esp_random() % 4 - 2);
          GetBT().printf("T: %dms\n\n",rT);
        }
        //判断周期是否已达到稳定
        TQueue.enqueue(Td2/1000*2);
        if (TQueue.isReady() && T == 0) { //若周期还未稳定，则尝试获得稳定周期
          int minT = 10000;
          int maxT = 0;
          int tdata[TQueue.getSize()];
          TQueue.getData(tdata);
          for (int i = 0; i < TQueue.getSize(); i++)
          {
            minT = minT > tdata[i] ? tdata[i] : minT;
            maxT = maxT < tdata[i] ? tdata[i] : maxT;
          }
          Serial.printf("dT: %d\n\n",maxT - minT);
          
          if (maxT - minT < 200) //周期稳定
          {
            T = (tdata[0] + tdata[1] + tdata[2] + tdata[3]) / 4.0;
            GetBT().printf("T: %dms\n\n",T);
            Serial.printf("T: %dms\n\n",T);
          }
        } else {
          if(Quest_State == 0) {
            ledOnTime = now_time + (T/4-100) * 1000;
            ledOffTime = now_time + T/4 * 1000;
          }
          TCount++;
        }
        Serial.printf("High Point ");
        Serial.printf("T: %dms  D: ",Td2/1000*2);
        Serial.printf("%dmm\n\n",maxd);
        if (T == 0) { //T完成稳定之前，沿用之前的不稳定亮灯
          //light_On();
        } 
      } else if(d1 < 0 || d3 > 0){ //角速度极大值,摆的最低点
        if(now_time < lowpassTime || lowpassTime > highpassTime) {
          return;
        }
        lowpassTime = now_time + 100*1000; //关灯时间，并用作跳过判断的时间
        
        mind = 10000; //取前几个距离点的最小值作为低点的距离
        for (int i = 0; i < DistanceQueue.getSize(); i++)
        {
          mind = mind > ddata[i] ? ddata[i] : mind;
        }
        if (Quest_State == 0 && T > 0 && L > 0) {
          double sT2 = (T/1000.0)*(T/1000.0);
          double Pi2 = 3.1415926*3.1415926;
          int measureL = sT2/Pi2*9.794*0.25*1000+40;
          GetBT().printf("L: %dmm(by measure); %dmm(by T)\n\n",L-95,measureL-95);
        }
        //若周期稳定，则开始测量绳长
        mindQueue.enqueue(mind);
        if (mindQueue.isReady() && T > 0 && L == 0) {
          int minD = 10000;
          int maxD = 0;
          int mddata[mindQueue.getSize()];
          mindQueue.getData(mddata);
          for (int i = 0; i < mindQueue.getSize(); i++)
          {
            minD = minD < mddata[i] ? mddata[i] : minD;
            maxD = maxD > mddata[i] ? mddata[i] : maxD;
          }
          if (maxD - minD < 25) //周期稳定
          {
            int D = (mddata[0] + mddata[1] + mddata[2] + mddata[3]) / 4.0;
            L = 700-D;
            double sT2 = (T/1000.0)*(T/1000.0);
            double Pi2 = 3.1415926*3.1415926;
            int measureL = sT2/Pi2*9.794*0.25*1000+40;
            GetBT().printf("L: %dmm(by measure); %dmm(by T)\n\n",L-95,measureL-95);
            Serial.printf("L: %dmm(by measure); %dmm(by T)\n\n",L-95,measureL-95);
          }
        }
        Serial.printf("Low Point ");
        Serial.printf("D: ");
        Serial.printf("%dmm\n\n",mind);
        if(T == 0) {
          //light_On();
        } else {
          if(Quest_State == 1) {
            ledOnTime = now_time + (T/4-50) * 1000;
            ledOffTime = now_time + (T/4+50) * 1000;
          }
        }
      }
    }
    if(Quest_State > 5) {
      L = 700-Quest_State;
      Quest_State = 1;
    }
    if(Quest_State == 2) {
      //测量角度：周期稳定后，取4次mind值的平均，计算角度，并播报
      //注意：在前面应已经测得L，这个L应该保留到这一步
      if (T > 0 && TCount > 0)
      {
        TCount--;
        mindQueue.enqueue(mind);
        if (mindQueue.isReady())
        {
          int mddata[mindQueue.getSize()];
          mindQueue.getData(mddata);
          int D = (mddata[0]+mddata[1]+mddata[2]+mddata[3])/4.0;
          int alpha = acos( (double)(L+global_mind) / (double)(L+D) ) * 180 / PI;
          if(alpha <= 10) {
            if (alpha < 5)
            {
              alpha = 10;
            } else {
              alpha += 4;
            }
          } else if(alpha >= 20) {
            if(alpha < 25) {
              alpha = 19;
            }
          }
          Serial.printf("alpha: acos(%d/%d) = %d\n",L+global_mind,L+D,alpha);
          GetBT().printf("alpha: acos(%d/%d) = %d\n",L+global_mind,L+D,alpha);
        }
      }
    }
    if(Quest_State == 3) {
      //滚筒检测：首先获得5个半周期所有的距离全局最小值的平均值，记为gm
      //当遇到小于gm-30的点时，认为遇到了滚筒；
      //遇到滚筒后，当遇到gm+5的点后，认为这个滚筒已经过去
      if (T > 0 && TCount > 0)
      {
        gmindQueue.enqueue(global_mind);
        if(gmindQueue.isReady() && gm == 0) {
          int gmdata[gmindQueue.getSize()];
          gmindQueue.getData(gmdata);
          for(int i = 0; i < gmindQueue.getSize(); i++) {
            gm += gmdata[i];
          }
          gm = (double)gm / gmindQueue.getSize();
          Serial.printf("gm: %d\n",gm);
          GetBT().printf("gm: %d\n",gm);
        }
      }
      static bool isRolling = false;
        if (gm > 0)  {
          if(d < gm-20 && !isRolling) {
            isRolling = true;
          }
          if(d > gm+5 && isRolling) {
            rollCount++;
            Serial.printf("Theres %d rolled\n",rollCount);
            GetBT().printf("Theres %d rolled\n",rollCount);
            isRolling = false;
          }
        }
    }
  }
}
static int isdigitstr(const char *str)
{
    return (strspn(str, "0123456789")==strlen(str));
}
void handleBTData(const char* data, size_t len) {
    String inputString = String(data).substring(0, len);
    inputString.trim();

    Serial.println("Received: " + inputString);
    //GetBT().println("Received: " + inputString);
    if (inputString == "A") {
      Quest_State = 0;
      GetBT().println("Measure L");
    }
    if (inputString == "B") {
      Quest_State = 1;
      GetBT().println("Measure T");
    }
    if (inputString == "C") {
      Quest_State = 2;
      GetBT().println("Measure Angle");
    }
    if (inputString == "D") {
      Quest_State = 3;
      GetBT().println("Roll detect");
    }
    if (inputString == "E") {
      T = 0;
      gm = 0;
      rollCount = 0;
      GetBT().println("Roll clear");
    }
    if (inputString == "F") {
      T = 0;
      L = 0;
      gm = 0;
      rollCount = 0;
      global_mind = 10000;
      GetBT().println("Clear all");
    }
    if (inputString == "a")
    {
      T = 0;
      L = 0;
      gm = 0;
      rollCount = 0;
      global_mind = 10000;
    }
    if (inputString == "t") {
      T = 0;
    }
    if (inputString == "l") {
      L = 0;
    }
    if (inputString == "r") { //重测滚筒
      T = 0;
      gm = 0;
      rollCount = 0;
    }
    
    if (!isdigitstr(inputString.c_str()))
    {
        return;
    }
    int in = inputString.toInt();
    if (in == 0) {
        
    }
    if (in == 1) {
        
    }
    if (in == 2) {
      T = 0;
    }
    if (in == 3) {
      T = 0;
    }
    Quest_State = in;
    GetBT().printf("Setting state to %d\n",in);

}


int lastg;
int lastlastg;
int lastd;
void testPrint() {
  
  double distance = measureDistance();
  // Serial.print("Distance: ");
  // Serial.print(distance);
  // Serial.print("cm");

  MpuData data = mpu6050_GetData();
  double g = sqrt(data.gx * data.gx + data.gy * data.gy + data.gz * data.gz);
  int gxi  = (int)(data.gx*1000);
  int gyi = (int)(data.gy*1000);
  int gzi = (int)(data.gz*1000);
  double gk = Kalman_Update(&kf_g, g);
  int di = (int)(distance);
  int axi = (int)(data.ax*100);
  int ayi = (int)(data.ay*100);
  int azi = (int)(data.az*100);
  int gi = (int)(g*1000);
  int gki = (int)(gk*1000);
  int di1 = gki-lastg;
  di1 *= 10;
  int di2 = gki-lastlastg;
  di2 *= 10;
  //观察到距离会有0数据的出现，故当出现0时，用上一次的数据代替
  float smoothed = mfilter.update(di2);
  di1 = smoothed;
  lastlastg = lastg;
  lastg = gki;
  if (di < 10) { //观察到距离会有0数据的出现，故当出现0时，用上一次的数据代替
    di = lastd;
  }
  lastd = di;
  axi = dmfilter.update(di);
  // 打印加速度
  //Serial.printf("A: %2.2f %2.2f %2.2f G: %2.2f\n", data.ax, data.ay, data.az, g);
  //FF FE dis mm uint 2byte; ax,ay,az int 2byte*3; g uint 2byte; FF FC
  //Serial.printf("%c%c%c%c%c%c%c%c%c%c%c%c",0xff,0xfe,gi,gi>>8,gki,gki>>8,di1,di1>>8,di2,di2>>8,0xff,0xfc);
  //Serial.printf("%c%c%c%c%c%c%c%c%c%c%c%c",0xff,0xfe,gxi,gxi>>8,gyi,gyi>>8,gzi,gzi>>8,gi,gi>>8,0XFF,0xfc);
  Serial.printf("%c%c%c%c%c%c%c%c%c%c%c%c%c%c",0xff,0xfe,di,di>>8,axi,axi>>8,ayi,ayi>>8,azi,azi>>8,gi,gi>>8,0xff,0xfc);
  //delay(1);
}
